/*
 *  Mikrokopter Flight control Serial Interface
 *  Copyright (C) 2010, CYPHY lab
 *  Inkyu Sa <i.sa@qut.edu.au>
 *
 *  http://wiki.qut.edu.au/display/cyphy
 *
 *
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "Comp_filter.h"

ros::Timer timer,timer1;
ros::WallTimer walltimer;

long g_nCounter=0;
long g_nCounter1=0;
long g_nCounter_h=0;
long g_nShapeCnt=0;
unsigned int g_throttle=0;
int g_barome_height=0;
char g_quad_state=ON_GROUND;

using namespace std;
stringstream ss (stringstream::in | stringstream::out);
string temp;
string message;
char buf[20];

extern unsigned char thrust[TABLE_SIZE];
Position_t g_CurPosition,g_PostPosition,g_GoalPosition;
Velocity_t g_W_Vel,g_B_Vel,g_B_ACC,g_B_ACC_pre,g_B_esti_Vel,g_B_predic_esti_Vel,g_B_goal_Vel,g_B_goal_Vel_;

int main(int argc, char **argv)
{
   int interval=0;
   int nLength;
   int cnt=3;
   
   ros::init(argc, argv, "ComplementaryFilter");

   printf("*************************************\n");
   printf("*  Press space bar to take off.     *\n");
   printf("*                                   *\n");
   printf("*  Make sure extern control on and  *\n");
   printf("*  Turn on the motors               *\n");
   printf("*                                   *\n");
   printf("*************************************\n");


   printf("*************************************\n");
   printf("*  Press ESC to land                *\n");
   printf("*                                   *\n");
   printf("*************************************\n");

   #if 0
   for(int i=0;i<cnt;i++)
   {
	   ros::WallDuration(1).sleep();	 // 3 seconds delay before taking off
	   printf("%d second\n",i+1);
   }
   #endif
   
   miko::CompFilter::CompFilter compfilter;
   compfilter.last_time = ros::Time::now();
   compfilter.last_time_ = ros::Time::now();
   ros::spin();
   return 0;
}

namespace miko
{
  CompFilter::CompFilter()
  {
    ROS_INFO ("Starting complementary Filter");
    ros::NodeHandle nh;
	pub = nh.advertise<cyphy_CF::mikoCmd>("mikoCmd", 100);
	marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker",1);
    pos_gui_pub = nh.advertise<std_msgs::Float32MultiArray>("pos_gui",1);
	//position2DSubscriber = nh.subscribe ("pose2D", 100, &CompFilter::pose2DCallback, this);
	//pole_position2DSubscriber = nh.subscribe ("pole_pose", 100, &CompFilter::pole_pose2DCallback, this);
	vision_pole_position2DSubscriber = nh.subscribe ("vision_pole_pose", 100, &CompFilter::vision_pole_pose2DCallback, this);
	mikoImuSubscriber = nh.subscribe ("mikoImu", 100, &CompFilter::mikoImuCallback, this);
	HeightSubscriber = nh.subscribe ("right_floor_altimeter/height", 100, &CompFilter::HeightCallback, this);
	HSubscriber = nh.subscribe ("height", 100, &CompFilter::HCallback, this);
	
	goalSubscriber = nh.subscribe ("goal", 100, &CompFilter::goalCallback, this);
	kbdSubscriber = nh.subscribe ("keycode", 100, &CompFilter::kbdCallback, this);
	odom_pub = nh.advertise<nav_msgs::Odometry>("odom",100);
	JoySubscriber = nh.subscribe("joy",100, &CompFilter::joyCallback, this);
	YawSubscriber = nh.subscribe ("yaw", 100, &CompFilter::YawCallback, this);


	
	g_B_goal_Vel.x=0;
	g_B_goal_Vel.y=0;
	g_B_goal_Vel.z=0;

	g_GoalPosition.x=INITIAL_POSE_X;
	g_GoalPosition.y=INITIAL_POSE_Y;
	//g_GoalPosition.z=0.6+HEIGHT_OFFSET; // 1m
	g_GoalPosition.z=0.6; // metre
	g_GoalPosition.theta=0;

	g_B_esti_Vel.z=0;


	//===================================================
    //   Visualization_msg initialization
    //===================================================
    
	shape = visualization_msgs::Marker::SPHERE;
	goal_marker.header.frame_id="/world";
	goal_marker.header.stamp = ros::Time::now();
	goal_marker.ns = "basic_shapes";
	goal_marker.id = g_nShapeCnt;
	goal_marker.type = shape;
	goal_marker.action = visualization_msgs::Marker::ADD;
	goal_marker.lifetime = ros::Duration();


	goal_marker.scale.x=0.05;
	goal_marker.scale.y=0.05;
	goal_marker.scale.z=0.05;

	// Set the color -- be sure to set alpha to something non-zero!
 	goal_marker.color.r=1.0f;
	//goal_marker.color.g=1.0f;
	goal_marker.color.b=1.0f;
	goal_marker.color.a=1.0f;
	//resource=r.get("package://test.STL");
	//goal_marker.mesh_resource = "package://test.STL";

	h_pid=0;
	p_error_height_last=0;
	p_error_height=0;
	i_error_height=0;
	d_error_height=0;

	p_error_vel_last=0;
	p_error_vel=0;
	i_error_vel=0;
	d_error_vel=0;

	flag1=false;
	//take_off=false;
	uRampGas=0;
	table_cnt=0;
	take_off_table_cmd=0;
	thrust_obtain_flag=FALSE;

	landing_thrust=0;
	landing_cnt=1;


	max_batt_in_air=155;
	curr_batt=0;
	batt_cnt=1;
	start_batt_samp=0;
	batt_avg_cnt=0;
	batt_sum=0;
	h_off_ratio=0;
	max_batt=160;
	get_first_gas=0;
	prev_height_gas=0;
	mikoCmd.yaw=0;
	continue_detect=0;
	
	
#if 0
	goal_marker.color.r = 0.0f;
	goal_marker.color.g = 1.0f;
	goal_marker.color.b = 0.0f;
	goal_marker.color.a = 1.0f;
#endif




/*
	pid_vel_x.initPid( 20 , 5 , 0 , 10 , -10 ); 
	pid_vel_y.initPid( 20 , 5 , 0 , 10 , -10 ); 
	pid_vel_z.initPid( 16 , 5 , 0 , 20 , 0 ); 
*/

//	pid_height.initPid(1,1,1,130,0);

    pos_gain[P_GAIN]=0.72;
    pos_gain[I_GAIN]=0.08;
    pos_gain[D_GAIN]=1.12;
    pos_gain[I_MAX]=0.5;
    pos_gain[I_MIN]=-0.5;

	vel_gain[P_GAIN]=19.95;
    vel_gain[I_GAIN]=0;
    vel_gain[D_GAIN]=0;
    vel_gain[I_MAX]=10;
    vel_gain[I_MIN]=-10;

	yaw_gain[P_GAIN]=6;
    yaw_gain[I_GAIN]=1;
    yaw_gain[D_GAIN]=2;
    yaw_gain[I_MAX]=10;
    yaw_gain[I_MIN]=-10;


	height_gain[P_GAIN]=21.7;
    height_gain[I_GAIN]=5;
    height_gain[D_GAIN]=13.3;
    height_gain[I_MAX]=15;
    height_gain[I_MIN]=-15;

	vel_term=0;

#if 0
	height_new_gain[P_GAIN]=0.3;
    height_new_gain[I_GAIN]=0.05;
    height_new_gain[D_GAIN]=0.2;
    height_new_gain[I_MAX]=0.2;
    height_new_gain[I_MIN]=0;

	height_vel_gain[P_GAIN]=16;
    height_vel_gain[I_GAIN]=5;
    height_vel_gain[D_GAIN]=0;
    height_vel_gain[I_MAX]=20;
    height_vel_gain[I_MIN]=0;

	h_bound_cnt=0;
#endif	

	height_new_gain[P_GAIN]=2;
    height_new_gain[I_GAIN]=0.2;
    height_new_gain[D_GAIN]=0.3;
    height_new_gain[I_MAX]=10;
    height_new_gain[I_MIN]=-10;

//	height_vel_gain[P_GAIN]=15.4;
//    height_vel_gain[I_GAIN]=5.6;
//    height_vel_gain[D_GAIN]=14.7;
//    height_vel_gain[I_MAX]=30;
//    height_vel_gain[I_MIN]=-30;

	height_vel_gain[P_GAIN]=21.7;
    height_vel_gain[I_GAIN]=5.6;
    height_vel_gain[D_GAIN]=23.8;
    height_vel_gain[I_MAX]=30;
    height_vel_gain[I_MIN]=-30;

	pid_yaw.initPid( yaw_gain[P_GAIN] , yaw_gain[I_GAIN] , yaw_gain[D_GAIN] , yaw_gain[I_MAX] , yaw_gain[I_MIN] ); 
	pid_x.initPid( pos_gain[P_GAIN] , pos_gain[I_GAIN] , pos_gain[D_GAIN] , pos_gain[I_MAX] , pos_gain[I_MIN] ); 
	pid_y.initPid( pos_gain[P_GAIN] , pos_gain[I_GAIN] , pos_gain[D_GAIN] , pos_gain[I_MAX] , pos_gain[I_MIN] ); 

	pid_z.initPid(0.6 , 0.1 , 0.5 , 0.5 , 0 );


/*

	pid_yaw.initPid( 6.0 , 1.0 , 2.0 , 0.3 , -0.3 ); 
	pid_x.initPid( 0.4 , 0.1 , 0.5 , 0.05 , -0.05 ); 
	pid_y.initPid( 0.4 , 0.1 , 0.5 , 0.05 , -0.05 ); 
	pid_z.initPid(0.3 , 0.05 , 0.2 , 0.3 , 0 );
*/



	pid_vel_x.initPid( vel_gain[P_GAIN] , vel_gain[I_GAIN] , vel_gain[D_GAIN] , vel_gain[I_MAX] , vel_gain[I_MIN] ); 
	pid_vel_y.initPid( vel_gain[P_GAIN] , vel_gain[I_GAIN] , vel_gain[D_GAIN] , vel_gain[I_MAX] , vel_gain[I_MIN] ); 
	pid_vel_z.initPid( 17 , 5 , 0 , 20 , 0 ); 
	

	circle_theta=0;


//	height_offset_init=115;
//	height_offset=115;

	quad_height=HEIGHT_OFFSET;

	h_pid_old=0;

	landing_throttle=0;

	height_res=0;
	height_cmd_ratio=0;
	height_res= MAX_HEIGHT - (MIN_HEIGHT+0.2); // 1.2
	height_cmd_ratio = 1.2/2.f; // 0.6

	joy_forward=0;
	joy_backward=0;
	joy_left=0;
	joy_right=0;
	   


    #if 0
	// UDP Client Initial 

	timeOut = 100; // ms

	remoteServAddr.sin_addr.s_addr=inet_addr("127.0.0.1"); // localhost.
	remoteServAddr.sin_family = AF_INET;
	remoteServAddr.sin_port = htons(REMOTE_SERVER_PORT);

	/* socket creation */
	sd = socket(AF_INET,SOCK_DGRAM,0);
	if(sd<0) 
	{
		printf("cannot open socket \n");
		exit(1);
	}
	else printf("open socket succeed!\n");

	/* bind any port */
	cliAddr.sin_family = AF_INET;
	cliAddr.sin_addr.s_addr = htonl(INADDR_ANY);
	cliAddr.sin_port = htons(0);

	rc = bind(sd, (struct sockaddr *) &cliAddr, sizeof(cliAddr));
	if(rc<0) 
	{
		printf("%s: cannot bind port\n");
		exit(1);
	}
	else printf("UDP socket binded!!\n");
	
	flags = 0;
	#endif

	

	
	
	// proportional gain, integral gain, derivative gain, integral upper limit, integral lower limit

//AnglePitch,AngleRoll,AngleYaw,ACCX,ACCY,ACCZ,esti_x,esti_y
    //fd=fopen("IROS_Vel_esti.data","w");
    fd=fopen("height_all.data","w");
    //fd=fopen("yaw_cmd","w");
	//fd_=fopen("cmd.data","w");
	fd_=fopen("pose_laser.data","w");
	fd_cmd=fopen("mk_cmd.data","w");
	
    fd_h=fopen("height","w");

	fd_mk=fopen("mk.data","w");

#ifdef GSPK_TCP_ON
	
	fd_gspk=fopen("gspk.data","w");
#endif
	fd_tbl=fopen("./Table/increase_tbl","r");
	if(!fd_tbl) printf("can't open increase tbl\n");

	// To create CSV type log file.
//	fprintf(fd_,"g_CurrentPosition.x,g_CurrentPosition.y,g_CurrentPosition.z,g_CurrentPosition.theta,yaw.cmd,index\n");
	fprintf(fd_,"g_CurrentPosition.x,g_CurrentPosition.y,g_CurrentPosition.z,g_CurrentPosition.theta,time\n");
//	fprintf(fd,"AnglePitch,AngleRoll,AngleYaw,AccX,AccY,AccZ,g_B_esti_Vel.x,g_B_esti_Vel.y,Index\n");
//	fprintf(fd,"g_B_Vel.x,g_B_Vel.y,g_B_esti_Vel.x,g_B_esti_Vel.y,Index\n");
//	fprintf(fd,"g_B_goal_Vel.x,g_B_goal_Vel.y,g_B_esti_Vel.x,g_B_esti_Vel.y,g_GoalPosition.x,g_GoalPosition.y,g_CurPosition.x,g_CurPosition.y,Index\n");
//	fprintf(fd,"yaw_cmd,Index\n");

//    fprintf(fd,"STICK_GAS,PRESSURE,LASER,ACCZ,index\n");
	//fprintf(fd,"Laser_Height,Throttle,PID,H_OFFSET,VEL_TERM,index\n");
	//fprintf(fd,"accZ,g_B_Vel.z,g_B_esti_Vel.z,height,index\n");
	fprintf(fd,"accZ,g_B_Vel.z,g_B_esti_Vel.z,Goal_z,height,index\n");
	//fprintf(fd,"g_B_ACC.z,MyAttitude.AnglePitch,g_CurPosition.z,index\n");


    //g_throttle,g_CurPosition.z,(int)MyAttitude.ACCZ,g_nCounter_h
	//fprintf(fd_h,"cmd_height,height,ACCZ,Index\n");
	fprintf(fd_h,"stick_gas,height,Time\n");
//	fprintf(fd,"esti_x,esti_y,Index\n");

	fprintf(fd_mk,"accx,accy,accz,esti_vx,esti_vy,esti_vz,pitch_angle,roll_angle,yaw_angle,stick_gas,time\n");
#ifdef GSPK_TCP_ON
	fprintf(fd_gspk,"%% System Time : gSpeak Time : X1 : Y1 : Z1 : NX1 : NY1 : NZ1 : OX1 : OY1 : OZ1 : X2 : Y2 : Z2 : NX2 : NY2 : NZ2 : OX2 : OY2 : OZ2\n");

	port = 50000;
    new_port = 0;

	fprintf(stderr, "Connecting to Master on %s\n",QUTGS01);
	
	master_server.Initialise(QUTGS01,port);
	fprintf(stderr, "Connected on port: %u\n",port);
	master_server >> message;
	new_port = atoi(message.c_str()); 
	if (port == 0)
	{	
		fprintf(stderr, "Maximum number of connections reached. Exiting.\n");
	}
	else
	{
		fprintf(stderr, "Reconnecting on port: %u\n", new_port);
		master_server.Initialise(QUTGS01,new_port);				
		fprintf(stderr, "Reconnected on port: %u\n",new_port);
		//fprintf(stdout, "%% System Time : gSpeak Time : X1 : Y1 : Z1 : NX1 : NY1 : NZ1 : OX1 : OY1 : OZ1 : X2 : Y2 : Z2 : NX2 : NY2 : NZ2 : OX2 : OY2 : OZ2\n");
	}
#endif

    //dynamic_reconfigure::Server<cyphy_CF::onlinegainConfig>::CallbackType f = boost::bind(&callback,_1, _2);
    dynamic_reconfigure::Server<cyphy_CF::onlinegainConfig>::CallbackType f = boost::bind(&CompFilter::reconfigureCB,this, _1, _2);
    srv.setCallback(f);

    timer= nh.createTimer(ros::Duration(0.030), &CompFilter::spin,this);  //50ms timer 20Hz
#ifdef GSPK_TCP_ON
    timer1= nh.createTimer(ros::Duration(0.010), &CompFilter::trajectory_gen,this);  //10ms timer for VICON
#endif
    ROS_INFO ("Serial setup finished");
  }

  // For autonomous taking off and landing.
  void CompFilter::spin(const ros::TimerEvent & e)
  {


	  int i=TABLE_SIZE;
#if 1
	  if(g_quad_state==ON_TAKING_OFF)
	  {
	      // Start to take off! 
	      // Get the thrust table
	      #if 0
	      if(table_cnt!=69)
		  {
//			  printf("value =%d\n",thrust[table_cnt]);
			  take_off_table_cmd=thrust[table_cnt];
			  table_cnt++;
	      }
		  #endif

//		  if(table_cnt!=60)  // 120


#if 1

          if(curr_batt)
          {
//			  if(table_cnt!=(int)(55*max_batt/curr_batt))  // 114
			  //if(table_cnt!=57)  // 114
			  if(table_cnt!=70)  // 114
//			  if(table_cnt!=60)  // 114
			  {
	//			  printf("value =%d\n",thrust[table_cnt]);
				  take_off_table_cmd+=2.5;
		          printf("takeoff=%d\n",take_off_table_cmd);
				  table_cnt++;
		      }			  
			  else
			  {
			      // Quad already took off!
			      // It's in the air!
			      
				  table_cnt=0;
				  g_quad_state=ON_AIR;
			  }
          }
#endif

	  }  
#endif
      if(quad_cmd==MOVING_TEST)
      //if(g_quad_state==MOVING_TEST)
	  {
		  
	      float fData;
		  g_GoalPosition.x=0.5*sin(DEG_TO_RAD(circle_theta));
		  g_GoalPosition.y=0.5*cos(DEG_TO_RAD(circle_theta));

		  printf("x=%f,y=%f\n",g_GoalPosition.x,g_GoalPosition.y);

		  circle_theta +=1.8; // 1 degree

		  #if 0
		  if(fscanf(fd_tbl,"%f",&fData)!=EOF)
		  {
			  g_GoalPosition.y=fData;
			  printf("g_GoalPosition.y=%f\n",g_GoalPosition.y);
		  }
		  else
		  {
			   //printf("Stop!\n");
			   //printf("g_GoalPosition.y=%f\n",g_GoalPosition.y);
		  }
		  #endif
		  
		  
	  }

	  if(quad_cmd==MAKING_TURN)
	  {
	     if(g_GoalPosition.theta<(360-THETA_TICK))
	     {
		     g_GoalPosition.theta=+THETA_TICK;  // It will take 9 sec to make a turn because 50ms*360/2= 9,000ms
	     }
		 else
		 {
		     // For the very last time, enforce g_GoalPosition as 0 = 360.
		     // Hope this work.
			 g_GoalPosition.theta = 0;
		 }
	  }

#if 1
	  if(landing_cnt==1)
	  {
	      //printf("===========================================================test\n");
		  landing_cnt++;

		  //if ESC key is pressed.
		  if(g_quad_state==ON_LANDING)
		  {
		     printf("Here1\n");
		     //only once obtain current thrust to land.
		     if(thrust_obtain_flag==FALSE)
		     {
		         landing_throttle=(double)mikoCmd.throttle;
				 thrust_obtain_flag=TRUE;
				 printf("Here2 thrust=%f\n",landing_throttle);
		     }

			 //After obtaining current thrust, start to decrease certain amount of steps.	
			 else
			 {
				 if(g_quad_state!=TOUCH_GROUND)
			     {
				     printf("Here3 thrust=%f\n",landing_throttle);
					 if(landing_throttle>=1)
					 {
					     landing_throttle -=LANDING_TICK;
						 printf("Here4 thrust=%f\n",landing_throttle);
						 mikoCmd.throttle=(unsigned char)landing_throttle;
					 }
					 else
					 { 
					 	landing_throttle =0;
						printf("Here5 thrust=%f\n",landing_throttle);
						mikoCmd.throttle=(unsigned char)landing_throttle;
					 }
			     }
			 }

				  
	      }
	  	}
	    else landing_cnt--;
#endif



  	}

  void CompFilter::trajectory_gen(const ros::TimerEvent & e)
  {
	  master_server >> message;
	  fprintf(fd_gspk, "%s\n",message.c_str());
  }


  
  CompFilter::~CompFilter()
  {
    fclose(fd);
	fclose(fd_h);
#ifdef GSPK_TCP_ON
	fclose(fd_gspk);
	//master_server.close();
#endif

	//fclose(fd_est);
    ROS_INFO ("Destroying FlightControl Interface");
  }

      void CompFilter::mikoImuCallback (const cyphy_CF::mikoImu& mikoImu)
	{

		 current_time_ = ros::Time::now();
	     //ROS_INFO ("mikoImuCallback works fine");

		 // need to implimentation 
		 // Publishing Yaw data as /IMU to improve performace of canonical scanmatcher.
		 
         MyAttitude.AnglePitch=mikoImu.anglePitch;
         MyAttitude.AngleRoll=mikoImu.angleRoll;
         MyAttitude.AngleYaw=mikoImu.angleYaw;

         MyAttitude.ACCX=mikoImu.linear_acceleration.x;
         MyAttitude.ACCY=mikoImu.linear_acceleration.y;
         MyAttitude.ACCZ=mikoImu.linear_acceleration.z;
		 g_throttle=(unsigned int)mikoImu.stick_throttle;
		 g_barome_height=mikoImu.barome_height; 


		 //====================================================================
         //   Battery level check every 5 sec, this loop has 30ms sampling time
         //====================================================================


		 if( (batt_cnt%(30*1) == 0) || start_batt_samp)
		 {
		   start_batt_samp=1;
		   if(mikoImu.batt)
		   {
		       if(batt_avg_cnt==10)
		       {
			       curr_batt = batt_sum/10;
				   batt_ratio = (float)max_batt/(float)curr_batt;
//				   printf("curr_batt=%d batt_ratio=%f \n",curr_batt,batt_ratio);
				   start_batt_samp=0;
				   batt_avg_cnt=0;
				   batt_sum=0;
		       }
			   else
			   {
				   batt_sum += mikoImu.batt;
				   batt_avg_cnt++;
				   //printf("batt_sum=%d %d\n",batt_sum,batt_avg_cnt);
			   }
		   }
		   
		 }
		 batt_cnt++;

		 g_B_ACC.x= (MyAttitude.ACCX-g*sin(MyAttitude.AnglePitch))/cos(MyAttitude.AnglePitch);
		 //cout<<"ACC X="<<g_B_ACC.x<<endl;

		 if(abs(g_B_ACC.x)>MAX_X_ACC) g_B_ACC.x=g_B_ACC_pre.x;

//         g_B_ACC.x = -(cos(DEG_TO_RAD(MyAttitude.AngleRoll/10.))*sin(DEG_TO_RAD(MyAttitude.AnglePitch/10.))*cos(DEG_TO_RAD(MyAttitude.AngleYaw))
//		 	+ sin(DEG_TO_RAD(MyAttitude.AngleRoll/10.))*sin(DEG_TO_RAD(MyAttitude.AngleYaw)))*((g_DebugData.Analog[THRUST]*0.2)/0.8);
//		 printf("g_B_ACC=%f\n",g_B_ACC.x);

		 
         g_B_ACC.y= (MyAttitude.ACCY+g*sin(MyAttitude.AngleRoll))/cos(MyAttitude.AngleRoll);
		//cout<<"ACC Y="<<g_B_ACC.y<<endl;
		if(abs(g_B_ACC.y)>MAX_Y_ACC) g_B_ACC.y=g_B_ACC_pre.y;
//         printf("g_B_ACC.x=%f\n",g_B_ACC.x);
//		 printf("g_B_ACC.y=%f\n",g_B_ACC.y);

//         g_B_ACC.z= MyAttitude.ACCZ/(cos(MyAttitude.AngleRoll)*cos(MyAttitude.AnglePitch));  // m/s^2
         //g_B_ACC.z= -MyAttitude.ACCZ; //m/s^2
         g_B_ACC.z= MyAttitude.ACCZ; //m/s^2

fprintf(fd_mk,"%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%f\n",
                                g_B_ACC.x,
                                g_B_ACC.y,
                                g_B_ACC.z,      
                                g_B_esti_Vel.x,
                                g_B_esti_Vel.y,
                                g_B_esti_Vel.z,
                                MyAttitude.AnglePitch,
                                MyAttitude.AngleRoll,
                                MyAttitude.AngleYaw,
                                g_throttle,
                                current_time_.toSec());

		 last_time_ = current_time_;

		 g_B_ACC_pre=g_B_ACC;

	}

 void CompFilter::vision_pole_pose2DCallback(const geometry_msgs::Pose2DConstPtr& vision_pole_pose2D)
{
	//ROS_INFO("vision callback");


    gettimeofday(&start_t, NULL);

	double curr_time_sys=(double)((start_t.tv_sec   * 1000000 + start_t.tv_usec  )/1000000.0);

	vision_duration = ((start_t.tv_sec   * 1000000 + start_t.tv_usec  ) - 
		(pre_t.tv_sec * 1000000 + pre_t.tv_usec)) / 1000000.0;
	//printf("Vision callback loop hz:\t %.f sec",vision_duration);
		//ROS_INFO("Vision callback loop hz:\t %.f s", vision_duration);

	current_time_pole_vision = ros::Time::now();
	g_CurPosition.x=(vision_pole_pose2D->x)*cos(MyAttitude.AnglePitch)*cos(MyAttitude.AngleRoll); // m 
	g_CurPosition.y=(vision_pole_pose2D->y)*cos(MyAttitude.AnglePitch)*cos(MyAttitude.AngleRoll);

	//cout<<"x ="<<g_CurPosition.x<<" y ="<<g_CurPosition.y<<endl;
	

		g_B_Vel.x = (double)((g_CurPosition.x - g_PostPosition.x)/VISION_T);  
        g_B_Vel.y = (double)((g_CurPosition.y - g_PostPosition.y)/VISION_T);  

		//g_B_Vel.x = (double)((g_CurPosition.x - g_PostPosition.x)/VISION_T);  
        //g_B_Vel.y = (double)((g_CurPosition.y - g_PostPosition.y)/VISION_T);  

	//cout<<"vel x = "<<g_B_Vel.x<<" vel y = "<<g_B_Vel.y<<endl;

	


		 //===================================================
         //   Position proportianl controller implementation.
         //===================================================

         ros::Duration dt;

		 if( (g_quad_state==ON_AIR) || (g_quad_state==ON_LANDING))
		 {
		    
				pid_x.setGains(pos_gain[P_GAIN], pos_gain[I_GAIN], pos_gain[D_GAIN], pos_gain[I_MAX], pos_gain[I_MIN]);  //p , i , d, i_max, i_min
				//g_B_goal_Vel.x=pid_x.updatePid(g_GoalPosition.x-g_CurPosition.x,ros::Duration().fromSec(VISION_T));
				g_B_goal_Vel.x=pid_x.updatePid(g_GoalPosition.x-g_CurPosition.x,ros::Duration().fromSec(VISION_T));
				//mikoCmd.pitch=pid_x.updatePid(g_GoalPosition.x-g_CurPosition.x,ros::Duration().fromSec(VISION_T));
				//printf("pid vel_x=%f\n",g_B_goal_Vel.x);

				if(g_B_goal_Vel.x >=MAX_X_VEL ) g_B_goal_Vel.x = MAX_X_VEL;
				if(g_B_goal_Vel.x <=-MAX_X_VEL ) g_B_goal_Vel.x = -MAX_X_VEL;

				pid_y.setGains(pos_gain[P_GAIN], pos_gain[I_GAIN], pos_gain[D_GAIN], pos_gain[I_MAX], pos_gain[I_MIN]);  //p , i , d, i_max, i_min
				//g_B_goal_Vel.y=pid_y.updatePid(g_CurPosition.y-g_GoalPosition.y,ros::Duration().fromSec(VISION_T));
				g_B_goal_Vel.y=pid_y.updatePid(g_CurPosition.y-g_GoalPosition.y,ros::Duration().fromSec(VISION_T));
				//mikoCmd.roll=pid_y.updatePid(g_CurPosition.y-g_GoalPosition.y,ros::Duration().fromSec(VISION_T));

				//printf("pid vel_y=%f\n",g_B_goal_Vel.y);

				if(g_B_goal_Vel.y >=MAX_Y_VEL ) g_B_goal_Vel.y = MAX_Y_VEL;
				if(g_B_goal_Vel.y <=-MAX_Y_VEL ) g_B_goal_Vel.y = -MAX_Y_VEL;

				

				//===================================================
				//  Complementary filter
				//===================================================

#if 1
				//g_B_esti_Vel.x = g_B_esti_Vel.x + (VISION_T)*((double)(K_X*(g_B_Vel.x-g_B_esti_Vel.x))+(double)g_B_ACC.x);
				//g_B_esti_Vel.y = g_B_esti_Vel.y + (VISION_T)*((double)(K_Y*(g_B_Vel.y-g_B_esti_Vel.y))+(double)g_B_ACC.y);

				//g_B_esti_Vel.x = g_B_esti_Vel.x + (VISION_T)*((double)(K_X*(g_B_Vel.x-g_B_esti_Vel.x))+(double)(g_B_ACC.x/5.0));
				//g_B_esti_Vel.y = g_B_esti_Vel.y + (VISION_T)*((double)(K_Y*(g_B_Vel.y-g_B_esti_Vel.y))+(double)(g_B_ACC.y/5.0));


				g_B_esti_Vel.x = g_B_esti_Vel.x + (VISION_T)*((double)(K_X*(g_B_Vel.x-g_B_esti_Vel.x))+(double)(g_B_ACC.x/5.0));
				g_B_esti_Vel.y = g_B_esti_Vel.y + (VISION_T)*((double)(K_Y*(g_B_Vel.y-g_B_esti_Vel.y))+(double)(g_B_ACC.y/5.0));

				g_B_esti_Vel.z = g_B_esti_Vel.z + (VISION_T)*((double)(5*(g_B_Vel.z-g_B_esti_Vel.z))+(double)g_B_ACC.z);
				g_B_esti_Vel.z *=-1;

				//cout<<"esti vx="<<g_B_esti_Vel.x<<" esti vy = "<<g_B_esti_Vel.y<<endl;
#endif

				
		     //if(current_time_.toSec()-last_time_.toSec()<0.055)
		     {

				#if 1
				//===================================================
				//   Velocity PI controller implementation.
				//===================================================

				//mikoCmd.pitch=Kp_vel*(g_B_goal_Vel.x-g_B_esti_Vel.x);
				pid_vel_x.setGains(vel_gain[P_GAIN], vel_gain[I_GAIN], vel_gain[D_GAIN], vel_gain[I_MAX], vel_gain[I_MIN]);  //p , i , d, i_max, i_min

				//printf("esti_vel=%f goal_vel=%f\n",g_B_esti_Vel.x,g_B_goal_Vel.x);
				
				//mikoCmd.pitch= pid_vel_x.updatePid(g_B_esti_Vel.x-g_B_goal_Vel.x,ros::Duration().fromSec(VISION_T));
				mikoCmd.pitch= pid_vel_x.updatePid(g_B_esti_Vel.x-g_B_goal_Vel.x,ros::Duration().fromSec(VISION_T));
			    //cmd[0]= (g_B_esti_Vel.x-g_B_goal_Vel.x)*19.5;

//				printf("mikoCmd.pitch=%d\n",mikoCmd.pitch);

				//printf("p cmd=%d\n",mikoCmd.pitch);
				#endif


				if(mikoCmd.pitch >=MAX_PITCH ) mikoCmd.pitch = MAX_PITCH;
				if(mikoCmd.pitch <=-MAX_PITCH ) mikoCmd.pitch = -MAX_PITCH;

				 //mikoCmd.roll=Kp_vel*(g_B_goal_Vel.y-g_B_esti_Vel.y);

#if 1
				pid_vel_y.setGains(vel_gain[P_GAIN], vel_gain[I_GAIN], vel_gain[D_GAIN], vel_gain[I_MAX], vel_gain[I_MIN]);  //p , i , d, i_max, i_min
				//mikoCmd.roll= pid_vel_y.updatePid(g_B_esti_Vel.y-g_B_goal_Vel.y,ros::Duration().fromSec(MK_T));
			    mikoCmd.roll = pid_vel_y.updatePid(g_B_esti_Vel.y-g_B_goal_Vel.y,ros::Duration().fromSec(VISION_T));
#endif
				//cmd[1]= (g_B_esti_Vel.y-g_B_goal_Vel.y)*19.5;
				//printf("pid pos_y=%f\n",mikoCmd.roll);
				//printf("r cmd=%d\n",mikoCmd.roll);

				if(mikoCmd.roll >=MAX_ROLL ) mikoCmd.roll = MAX_ROLL;
				if(mikoCmd.roll <=-MAX_ROLL ) mikoCmd.roll = -MAX_ROLL;

				
		     }

	//cout<<"g_B_ACC.x="<<g_B_ACC.x<<endl;
/*	
				fprintf(fd_mk,"%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%f\n",
				g_B_ACC.x,
				g_B_ACC.y,
				g_B_ACC.z,	
				g_B_esti_Vel.x,
				g_B_esti_Vel.y,
				g_B_esti_Vel.z,
				MyAttitude.AnglePitch,
				MyAttitude.AngleRoll,
				MyAttitude.AngleYaw,
				g_throttle,
				curr_time_sys);
*/


		}

		//===================================================
		//   Comments for the MK quadrotor.
		//===================================================
		/*
		In order to adjust Mikrokopter flight controller.
		Mikrokopter doesn't have concept of any axis. It only has pitch,roll and yaw.
		Red rod is the front and the opposite one is the rear. Pitch(Nick) means forwards and backwrads
		movements. In contrast, Roll means left and right movements.
		When pushing all the way up the pitch stick(Right stick on mode2), 
		"StickNick" value(516) is transmitted
		to the FlightControl(FC). This implies that moving forward as fastest speed.
		Actually it means maximum angular velocity of pitch. 
		When pulling, opposingly all the way down the pitch stick, "StickNick" value(-592) is transmitted
		to the FlightControl. This implies that moving backward as fastest speed.
		On the other hand, When pushing all the way right the Roll stick, "StickRoll" value(-516) is transmitted
		to the FC. when pushing all the way left the Roll stick, 
		"StickRoll" value(512) is transmitted to the FC.

		When pushing all the way to the right of Yaw stick, "StickYaw" value(125) is transmitted. Al the way to the left of Yaw stick is (-125).
	     */
	     
		 mikoCmd.pitch*=1;
 		 mikoCmd.roll*=1;
 		 mikoCmd.throttle*=1;
		 
		 //printf("mikoCmd.throttle=%d\n",mikoCmd.throttle);

		 //mikoCmd.yaw=yaw_ctrl/YAW_SCALE_FACTOR;
		 //mikoCmd.yaw=0;
		 //printf("mikoCmd.yaw=%d\n",mikoCmd.yaw);
		 //mikoCmd.throttle=MAX_THROTTLE;

         
	 

		 




#if 0 // Move this code to height callback
         switch(g_quad_state)
         {
		 	case ON_AIR:
//			mikoCmd.throttle=height_offset+h_pid_old; //position PID, It works well however not stable.
			
			//printf(" height_offset = %d %f\n",(int)((float)((max_batt_in_air/curr_batt))*(float)height_offset),(float)((max_batt_in_air/curr_batt)));
#if 0
			if(curr_batt)
			{
				h_off_ratio=(float)max_batt_in_air/(float)curr_batt;
				height_offset = h_off_ratio*(float)height_offset_init;
				printf(" height_offset = %d %.4f\n",(int)height_offset,h_off_ratio);
	        }
#endif

			// Obtain the first gas only once after taking off.
            // Because take_off_table_cmd value changes based on battery level.
            // For example, if battery level is high, this value is low and vice versa.

#if 1
	        if(!get_first_gas)
	        {
		        height_offset = take_off_table_cmd;
				get_first_gas=true;
	        }
#endif

			#if 0
			else
			{
				height_offset = prev_height_gas;
			}
			#endif
			
				
	           
			//mikoCmd.throttle=height_offset+(int)h_pid_old;  // for CF velocity n position.
            //fTemp = ((float)height_offset+(float)h_pid)/batt_ratio;
			//fTemp = (float)(height_offset/(cos(MyAttitude.AnglePitch)*cos(MyAttitude.AngleRoll)))+(float)h_pid;

			fTemp = (float)height_offset+(float)h_pid;

			fTemp=fTemp/(cos(MyAttitude.AnglePitch)*cos(MyAttitude.AngleRoll));
			
			//printf("fTemp =%f\n",fTemp);
			mikoCmd.throttle=(unsigned char)fTemp;
			//mikoCmd.throttle=(int)h_pid;  // for CF velocity n position.
//			mikoCmd.throttle=(int)h_pid;  // for CF velocity n position.
				
			//printf("height_offset=%d h_pid=%f thrust=%d fTemp=%f\n",height_offset,h_pid,mikoCmd.throttle,fTemp);
			break;
			case ON_TAKING_OFF:
			mikoCmd.throttle=take_off_table_cmd;
			break;
			case TOUCH_GROUND:
			mikoCmd.throttle=60;
			break;
         }
#endif
		 
			
		 #if 0 // Move this code to height callback

		 if(mikoCmd.throttle>=165) mikoCmd.throttle=165;
		 if(mikoCmd.throttle<0) mikoCmd.throttle=0;

		 #endif
	
		 //mikoCmd.pitch=0;
		 //mikoCmd.roll=0;
		 //mikoCmd.yaw = yaw_ctrl;
		mikoCmd.yaw = 0;

#if 1
		 if(abs(mikoCmd.pitch) >40 || abs(mikoCmd.roll) >40)
		 {
			 printf("Pitch or Roll cmd are too big!! Ignore this command for safety reason\n");
		     mikoCmd.pitch=0;
			 mikoCmd.roll=0;
			 pub.publish(mikoCmd);
			 //return;
		 }


		 else pub.publish(mikoCmd);

#endif

		 cout<<"pitch cmd = "<<mikoCmd.pitch<<" roll cmd = "<<mikoCmd.roll<<endl; 
		 //printf("\rpitch cmd = %d roll cmd = %d",mikoCmd.pitch,mikoCmd.roll);
		 //fflush(stdout);


        pos_msg.data.push_back(g_GoalPosition.x);	
		pos_msg.data.push_back(g_CurPosition.x);
		pos_msg.data.push_back(g_GoalPosition.y);	
		pos_msg.data.push_back(g_CurPosition.y);
		pos_msg.data.push_back(g_GoalPosition.z);
  		pos_msg.data.push_back(g_CurPosition.z);
		pos_gui_pub.publish(pos_msg);
		pos_msg.data.clear();
		


//AnglePitch,AngleRoll,AngleYaw,AccX,AccY,AccZ,index\n	
// 7 elements
#if 0
		fprintf(fd,"%f,%f,%f,%f,%f,%f,%f,%f,%d\n",
			MyAttitude.AnglePitch,    MyAttitude.AngleRoll,    MyAttitude.AngleYaw,
			MyAttitude.ACCX,    MyAttitude.ACCY,    MyAttitude.ACCZ,
			g_B_esti_Vel.x,g_B_esti_Vel.y,
			g_nCounter);
	    g_nCounter++;
#endif

#if 0
		fprintf(fd,"%f,%f,%f,%f,%d\n",
            g_B_Vel.x,g_B_Vel.y,
			g_B_esti_Vel.x,g_B_esti_Vel.y,
			g_nCounter);
	    g_nCounter++;
#endif

#if 0
		fprintf(fd,"%d,%d\n",mikoCmd.yaw,g_nCounter);
	    g_nCounter++;
#endif


#if 0
fprintf(fd,"%f,%f,%f,%f,%f,%f,%f,%f,%d\n",
            g_B_goal_Vel.x,g_B_goal_Vel.y,
			g_B_esti_Vel.x,g_B_esti_Vel.y,
			g_GoalPosition.x,g_GoalPosition.y,
			g_CurPosition.x,g_CurPosition.y,
			g_nCounter);
	    g_nCounter++;
#endif
	//printf("accZ = %f velZ = %f estiVel_Z =%f height=%f\n",g_B_ACC.z,g_B_Vel.z,g_B_esti_Vel.z,g_CurPosition.z);
	//fprintf(fd,"%f,%f,%f,%f,%d\n",g_B_ACC.z,g_B_Vel.z,g_B_esti_Vel.z,g_CurPosition.z,g_nCounter);
	//printf("g_barome_height=%d\n",g_barome_height);

	fprintf(fd,"%f,%f,%f,%f,%f,%f\n",
				g_B_ACC.z,
				g_B_Vel.z,
				g_B_esti_Vel.z,
				g_GoalPosition.z,
				g_CurPosition.z,
				curr_time_sys);

	//fprintf(fd,"%f,%d,%d,%d,%f,%d\n", g_CurPosition.z , (int)mikoCmd.throttle, (int)h_pid, (int)height_offset,vel_term, g_nCounter);


	

	//printf("%d\n",g_throttle);
#if 0
	fprintf(fd,"%d,%d,%f,%f,%d\n", 
	          g_throttle,  
	          g_barome_height , 
	          g_CurPosition.z , 
	          MyAttitude.ACCZ,
	          g_nCounter);
#endif

	//fprintf(fd,"%f,%f,%f,%d\n",g_B_ACC.z,MyAttitude.AnglePitch,g_CurPosition.z,g_nCounter);
	g_nCounter++;


		
#if 0
		fprintf(fd,"%f,%f,%d\n",g_B_esti_Vel.x,g_B_esti_Vel.y,g_nCounter);
	    g_nCounter++;
#endif

		//fprintf(fd_cmd,"%d,%d,%f\n",mikoCmd.pitch,mikoCmd.roll,current_time_.toSec());

	    fprintf(fd_cmd,"%d,%d,%f\n",mikoCmd.pitch,mikoCmd.roll,curr_time_sys);

#if 1
		fprintf(fd_,"%f,%f,%f,%f,%f\n",
			g_CurPosition.x,    
			g_CurPosition.y, 
			g_CurPosition.z, 
			g_CurPosition.theta, 
			curr_time_sys);

	    g_nCounter1++;
#endif
		 g_PostPosition = g_CurPosition;
		 last_time_pole_vision = current_time_pole_vision;

		 gettimeofday(&end_t, NULL);
		double vision_dur = ((end_t.tv_sec   * 1000000 + end_t.tv_usec  ) - 
		(start_t.tv_sec * 1000000 + start_t.tv_usec)) / 1000.0;
		//ROS_INFO("Vision callback dur:\t %.f ms", vision_dur);
		pre_t = end_t;

#if 0
		 ROS_INFO ("%f %f %f %f %f %f",
		 	MyAttitude.AnglePitch,
		 	MyAttitude.AngleRoll,
		 	MyAttitude.AngleYaw,
		 	MyAttitude.ACCX,
		 	MyAttitude.ACCY,
		 	MyAttitude.ACCZ);
#endif
}	

 
	void CompFilter::HeightCallback(const sensor_msgs::Imu& msg)
    {
		 //double eP,eI,eD;
         //ROS_INFO("HeightCallback");

		 current_time__ = ros::Time::now();

		 g_CurPosition.z=msg.orientation.z;
		 //printf("before h=%f\n",g_CurPosition.z);
         // Compensate the height measurement using the pitch and roll angle.
		 g_CurPosition.z*=cos(MyAttitude.AnglePitch)*cos(MyAttitude.AngleRoll);

//		 printf("after h=%f\n",g_CurPosition.z);


         // Altitude PID control, fluctuated.
		 #if 1
//		 g_CurPosition.z=msg.orientation.z;

		 //printf("height =%f\n",g_CurPosition.z);

#if 0
         if(uRampGas>=100)
		 {
			 uRampGas =100;
			 h_pid=uRampGas+PID_Height(g_GoalPosition.z-g_CurPosition.z);
         }	 
		 else
		 {
			 uRampGas = uRampGas +1;
			 h_pid=uRampGas;
		 }
#endif

//         h_pid=PID_Height(g_GoalPosition.z-g_CurPosition.z);

	     #if 1
		 if(g_quad_state==ON_AIR)
		 {
			 //h_pid_old =PID_height(g_GoalPosition.z-g_CurPosition.z,height_gain[P_GAIN],height_gain[I_GAIN],height_gain[D_GAIN],\
		     //height_gain[I_MAX],height_gain[I_MIN],(current_time__.toSec()-last_time__.toSec())); //error,  p,  i,  d,  i_max,  i_min,  t
		 #endif
		 }

		 //printf("h_pid_old =%d\n",(int)h_pid_old);



		 //printf("new=%f old=%f\n",h_pid,htest);



		 //if(h_pid>=130) h_pid=130;
		 //if(h_pid<=0) h_pid=0;



		 
		 #endif
		 
		 
		 //h_pid=pid_height.updatePid(-(g_CurPosition.z-g_GoalPosition.z), current_time__-last_time__);
		 //pid_height.getCurrentPIDErrors(&eP,&eI,&eD);

		 //printf("eP=%f eI=%f eD=%f\n",eP,eI,eD);

		 //printf("height =%f cmd=%f\n",g_CurPosition.z,h_pid);
		 

		 #if 1

		 //===================================================
         //   Calculate z axis velocity 
         //===================================================
         //g_B_Vel.z = (g_CurPosition.z - g_PostPosition.z)/LASER_T;  // 100ms sampling time.
         g_B_Vel.z = (g_CurPosition.z - g_PostPosition.z)/(current_time__.toSec()-last_time__.toSec());

		 g_B_Vel.z *=-1; // reverse axis, downward is the positive direction of our quadrotor.

		 if(g_B_Vel.z >=MAX_Z_VEL ) g_B_Vel.z = MAX_Z_VEL;
		 if(g_B_Vel.z <=-MAX_Z_VEL ) g_B_Vel.z = -MAX_Z_VEL;

         //printf("g_CurPosition.z=%f g_PostPosition.z=%f\n",g_CurPosition.z,g_PostPosition.z);
		 //printf("b_vel=%f\n",g_B_Vel.z);

		 //===================================================
         //   Height Position proportianl controller implementation.
         //===================================================

		 //g_B_goal_Vel.z=pid_z.updatePid(g_CurPosition.z-g_GoalPosition.z,current_time__-last_time__);
//		 if(g_quad_state==ON_AIR)

#if 1
		 if(g_quad_state==ON_AIR)
		 {
			//ros::Duration d(0.1);
//			printf("Start taking off!\n");
	        if(current_time__.toSec()-last_time__.toSec() < 0.13)
	        {
				pid_z.setGains(height_new_gain[P_GAIN], height_new_gain[I_GAIN], height_new_gain[D_GAIN], height_new_gain[I_MAX], height_new_gain[I_MIN]);  //p , i , d, i_max, i_min
				//float rospid_z_=pid_z.updatePid(g_CurPosition.z-g_GoalPosition.z,current_time__-last_time__);
				//float rospid_z=pid_z.updatePid(g_CurPosition.z-g_GoalPosition.z,d);
				g_B_goal_Vel.z=pid_z.updatePid(g_CurPosition.z-g_GoalPosition.z,current_time__-last_time__);

				//float z_height =PID_height(g_GoalPosition.z-g_CurPosition.z,
				//height_new_gain[P_GAIN],  height_new_gain[I_GAIN],  height_new_gain[D_GAIN],  
				//height_new_gain[I_MAX],  height_new_gain[I_MIN],  current_time__.toSec()-last_time__.toSec());

				//printf("g_B_goal_Vel.z=%f , z_height=%f\n",g_B_goal_Vel.z,z_height);

				//float duration_ = current_time__.toSec()-last_time__.toSec();
	//			printf("pid duration = %f error =%f g_B_goal_Vel.z = %f rospid_z = %f rospid_z_=%f\n",duration_,g_CurPosition.z-g_GoalPosition.z,g_B_goal_Vel.z,rospid_z,rospid_z_);
				//printf("error =%f g_B_goal_Vel.z = %f\n",g_GoalPosition.z-g_CurPosition.z,g_B_goal_Vel.z);
	        }
			
		 }
#endif

		 //printf("g_B_goal_Vel.z=%f \n",g_B_goal_Vel.z);

		 if(g_B_goal_Vel.z >=MAX_Z_VEL ) g_B_goal_Vel.z = MAX_Z_VEL;
		 if(g_B_goal_Vel.z <=-MAX_Z_VEL ) g_B_goal_Vel.z = -MAX_Z_VEL;


		 #if 0

		 g_B_goal_Vel.z=PID_Height(g_GoalPosition.z-g_CurPosition.z);

		 //g_B_goal_Vel.z=Kp_height_pos*(g_GoalPosition.z-g_CurPosition.z);
		 
		 if(g_B_goal_Vel.z >=MAX_Z_VEL ) g_B_goal_Vel.z = MAX_Z_VEL;
		 if(g_B_goal_Vel.z <=-MAX_Z_VEL ) g_B_goal_Vel.z = -MAX_Z_VEL;

		 
		 #endif
		 g_PostPosition.z = g_CurPosition.z;

		 #endif
		 

		 #if 1
		 //printf("%f,%f,%d,%d\n",h_pid,g_CurPosition.z,(int)uRampGas,g_nCounter_h);
		 //printf("            Height=%f\n",g_CurPosition.z);
		 fprintf(fd_h,"%d,%f,%f\n",g_throttle,g_CurPosition.z,current_time__.toSec());
		 #endif
		 
		 g_nCounter_h++;

		 // Check whether the quad touch the ground or not.

//		 if((g_quad_state==ON_LANDING)&&(g_CurPosition.z<=(HEIGHT_OFFSET+TOUCH_THRESHOLD)))
		 if((g_quad_state==ON_LANDING)&&(g_CurPosition.z<=(0.15)))
		 {
			 printf("touch the ground\n");
			 g_quad_state=TOUCH_GROUND;
		 }
		 last_time__ = current_time__;
		 
	}

	void CompFilter::HCallback(const std_msgs::Float32::ConstPtr& msg)
	{
	  //ROS_INFO("HCallback");
	  //cout<<msg->data<<endl;
	  current_time__ = ros::Time::now();
      g_CurPosition.z=msg->data;
	  //printf("before h=%f\n",g_CurPosition.z);
      // Compensate the height measurement using the pitch and roll angle.
	  g_CurPosition.z*=cos(MyAttitude.AnglePitch)*cos(MyAttitude.AngleRoll);
      //if((g_quad_state==ON_LANDING)&&(g_CurPosition.z<=(HEIGHT_OFFSET+TOUCH_THRESHOLD)))
      if((g_quad_state==ON_LANDING)&&(g_CurPosition.z<=(0.18)))
	  {
		   printf("touch the ground\n");
		   g_quad_state=TOUCH_GROUND;
	  }

	fprintf(fd_h,"%d,%f,%f\n",g_throttle,g_CurPosition.z,current_time__.toSec());

	 //===================================================
     //   Calculate z axis velocity 
     //===================================================
     //g_B_Vel.z = (g_CurPosition.z - g_PostPosition.z)/LASER_T;  // 100ms sampling time.
     g_B_Vel.z = ((g_CurPosition.z - g_PostPosition.z)/LASER_T);

	 g_B_Vel.z *=-1; // reverse axis, downward is the positive direction of our quadrotor.

	 if(g_B_Vel.z >=MAX_Z_VEL ) g_B_Vel.z = MAX_Z_VEL;
	 if(g_B_Vel.z <=-MAX_Z_VEL ) g_B_Vel.z = -MAX_Z_VEL;

	 g_PostPosition.z = g_CurPosition.z;
	 last_time__ = current_time__;

     
	}

	void CompFilter::YawCallback(const std_msgs::Float32::ConstPtr& msg)
	{
		//cout<<"Yaw callback"<<endl;
		g_CurPosition.theta=msg->data;	//Degree
		//cout<<"yaw = "<<g_CurPosition.theta<<endl;
	
	}

	void CompFilter::goalCallback (const geometry_msgs::PoseStamped& msg)
	{
		 ROS_INFO("Goal received %f %f", msg.pose.position.x,msg.pose.position.y);
         goal_marker.pose.position.x = msg.pose.position.x;
		 goal_marker.pose.position.y = msg.pose.position.y;
		 goal_marker.pose.position.z = msg.pose.position.z;

		 g_GoalPosition.x = msg.pose.position.x;
         g_GoalPosition.y = -msg.pose.position.y;
		 goal_marker.id = g_nShapeCnt;
		 marker_pub.publish(goal_marker);		
		 g_nShapeCnt++;
		 
		 
	}

	void CompFilter::kbdCallback (const cyphy_kbd::KeyCode& msg)
	{
		 char data=msg.data;
//		 ROS_INFO("key_value=0x%02x\n",data);


		 switch(data)
		 {
		      case KEYCODE_SPACEBAR:
			  	printf("Start taking off!\n");
				g_quad_state=ON_TAKING_OFF;	
				break;
	 	      case KEYCODE_ESC:
			  	printf("Start landing!\n");
                g_quad_state=ON_LANDING;	 	        
	 	        break;

				 //  Our world coordinate
				 //  x
				 //  ^
				 //  |
				 //  |
		 		 //  |
		 		 //  -------->y

				 //  rviz world coordinate
				 //          x
				 //          ^
				 //          |
				 //          |
		 		 //          |
		 		 //  y<-------
		 		 //
		 		 //  q ---- w-----e
		 		 //  |            |
		 		 //  |            |
		 		 //  |            |
		 		 //  a------------s

	 		  case KEYCODE_a:
           
				g_GoalPosition.x=0.0;
				g_GoalPosition.y=0.0;
				break;

			  case KEYCODE_w:
                
				g_GoalPosition.x=1.0;
				g_GoalPosition.y=1.0;
				break;
			  case KEYCODE_q:
           
				g_GoalPosition.x=1.0;
				g_GoalPosition.y=0.0;
				break;
			  case KEYCODE_s:
			  	// quad_cmd= MOVING_TEST;
             	g_GoalPosition.x=0.0;
				g_GoalPosition.y=1.0;
				break;
				
			  case KEYCODE_r:
			  	g_GoalPosition.theta+=2; // Degree
             	//g_GoalPosition.x=1.0;
				//g_GoalPosition.y=0.5;
				break;

			  case KEYCODE_9:
			  	g_GoalPosition.theta=90; // Degree
             	//g_GoalPosition.x=1.0;
				//g_GoalPosition.y=0.5;
				break;

			  case KEYCODE_e:
//             	g_GoalPosition.x=1.0;
//				g_GoalPosition.y=1.0;
				break;
 			  case KEYCODE_c:
			  	
			  	//printf("Goal theta=%f\n",g_GoalPosition.theta);
			  	//g_quad_state = MAKING_CIRCLE;
				break;

			  case KEYCODE_f:
			  	if(g_GoalPosition.z>=MAX_HEIGHT) g_GoalPosition.z=MAX_HEIGHT;
				else g_GoalPosition.z +=HEIGHT_TICK;
				cout<<"Goal Z = "<<g_GoalPosition.z<<endl;
				break;
			  case KEYCODE_v:
			  	if(g_GoalPosition.z<=MIN_HEIGHT) g_GoalPosition.z=MIN_HEIGHT;
				else g_GoalPosition.z -=HEIGHT_TICK;
				cout<<"Goal Z = "<<g_GoalPosition.z<<endl;
				break;

			  case KEYCODE_t:
			  	quad_cmd = MAKING_TURN;
				break;
			  	
		 	}//swich
   }

	void CompFilter::reconfigureCB(cyphy_CF::onlinegainConfig &config, uint32_t level)
    {
	    pos_gain[P_GAIN]=config.Position_P;
	    pos_gain[I_GAIN]=config.Position_I;
	    pos_gain[D_GAIN]=config.Position_D;

		vel_gain[P_GAIN]=config.Velocity_P;
	    vel_gain[I_GAIN]=config.Velocity_I;
		
		yaw_gain[P_GAIN]=config.Yaw_P;
	    yaw_gain[I_GAIN]=config.Yaw_I;
	    yaw_gain[D_GAIN]=config.Yaw_D;

		//height_gain[P_GAIN]=config.Height_P;
	    //height_gain[I_GAIN]=config.Height_I;
	    //height_gain[D_GAIN]=config.Height_D;

		height_new_gain[P_GAIN]=config.Height_P;
	    height_new_gain[I_GAIN]=config.Height_I;
	    height_new_gain[D_GAIN]=config.Height_D;

		height_vel_gain[P_GAIN]=config.Height_vel_P;
	    height_vel_gain[I_GAIN]=config.Height_vel_I;
		height_vel_gain[D_GAIN]=config.Height_vel_D;
		
		
//	    height_offset = config.Height_OFFSET;

		//printf("%.2f %.2f %.2f\n",pos_gain[P_GAIN],pos_gain[I_GAIN],pos_gain[D_GAIN]);
	}

/*
	double CompFilter::PID_Height(double error)
	{
		  p_error = error;
		  //printf("error=%f\n",p_error);
		  // Calculate proportional contribution to command
		  p_term = KP_H * p_error;

		  // Calculate the integral error
//		  i_error = i_error + dt.toSec() * p_error;
 		  i_error = i_error + LASER_T * p_error;
		  //printf("i_error=%f\n",i_error);

		  //Calculate integral contribution to command
		  i_term = KI_H * i_error;
		
		  // Limit i_term so that the limit is meaningful in the output
		  if (i_term > 130)
		  {
			//printf("1\n");
		    i_term = 130;
		    i_error=i_term/KI_H;
		  }
		  else if (i_term < 0)
		  {
		  	//printf("2\n");
		    i_term = 0;
		    i_error=i_term/KI_H;
		  }

		  // Calculate the derivative error
		  //if (dt.toSec() != 0)
		  //{
//		    d_error = (p_error - p_error_last) / dt.toSec();

           // For the safety reason, ignore the first sample of the D term error.
           if(!flag1)
		   {
			   d_error=0;
			   flag1=true;
           }
		   else d_error = (p_error - p_error_last) / LASER_T;
//		  printf("p_error=%f p_error_last=%f\n",p_error,p_error_last);

		    //printf("d_error=%f\n",d_error);
		    p_error_last = p_error;
		  //}
		  // Calculate derivative contribution to command
		  d_term = KD_H * d_error;
		  double cmd = p_term + i_term + d_term;
		  //printf("cmd =%f\n",cmd);
		  return cmd;
	}
*/


      double p_error_height_last;
      double p_error_height,d_error_height,i_error_height;
		  double p_term_height, d_term_height, i_term_height;

      double p_error_vel_last;
      double p_error_vel,d_error_vel,i_error_vel;
		  double p_term_vel, d_term_vel, i_term_vel;


	double CompFilter::PID_height(double error,double p,double i,double d,double i_max,double i_min,double t)
	{
		  p_error_height = error;
		  //printf("error=%f\n",p_error);
		  // Calculate proportional contribution to command
		  p_term_height = p * p_error_height;

		  // Calculate the integral error
//		  i_error = i_error + dt.toSec() * p_error;
 		  i_error_height = i_error_height + t * p_error_height;
		  //printf("i_error=%f\n",i_error);

		  //Calculate integral contribution to command
		  i_term_height = i * i_error_height;
		
		  // Limit i_term so that the limit is meaningful in the output
		  if (i_term_height > i_max)
		  {
			//printf("1\n");
		    i_term_height = i_max;
		    i_error_height=i_term_height/i;
		  }
		  else if (i_term_height < i_min)
		  {
		  	//printf("2\n");
		    i_term_height = i_min;
		    i_error_height=i_term_height/i;
		  }

		  // Calculate the derivative error
		  //if (dt.toSec() != 0)
		  //{
//		    d_error = (p_error - p_error_last) / dt.toSec();

           // For the safety reason, ignore the first sample of the D term error.
           if(!flag1)
		   {
			   d_error_height=0;
			   flag1=true;
           }
		   else d_error_height = (p_error_height - p_error_height_last) / t;
//		  printf("p_error=%f p_error_last=%f\n",p_error,p_error_last);

		    //printf("d_error=%f\n",d_error);
		    p_error_height_last = p_error_height;
		  //}
		  // Calculate derivative contribution to command
		  d_term_height = d * d_error_height;
		  double cmd = p_term_height + i_term_height + d_term_height;
		  //printf("cmd =%f\n",cmd);
		  return cmd;
	}

	double CompFilter::PID_vel_height(double error,double p,double i,double d,double i_max,double i_min,double t)
	{
		  p_error_vel = error;
		  //printf("error=%f\n",p_error);
		  // Calculate proportional contribution to command
		  p_term_vel = p * p_error_vel;

		  // Calculate the integral error
//		  i_error = i_error + dt.toSec() * p_error;
 		  i_error_vel = i_error_vel + t * p_error_vel;
		  //printf("i_error=%f\n",i_error);

		  //Calculate integral contribution to command
		  i_term_vel = i * i_error_vel;
		
		  // Limit i_term so that the limit is meaningful in the output
		  if (i_term_vel > i_max)
		  {
			//printf("1\n");
		    i_term_vel = i_max;
		    i_error_vel=i_term_vel/i;
		  }
		  else if (i_term_vel < i_min)
		  {
		  	//printf("2\n");
		    i_term_vel = i_min;
		    i_error_vel=i_term_vel/i;
		  }

		  // Calculate the derivative error
		  //if (dt.toSec() != 0)
		  //{
//		    d_error = (p_error - p_error_last) / dt.toSec();

           // For the safety reason, ignore the first sample of the D term error.
           if(!flag1)
		   {
			   d_error_vel=0;
			   flag1=true;
           }
		   else d_error_vel = (p_error_vel - p_error_vel_last) / t;
//		  printf("p_error=%f p_error_last=%f\n",p_error,p_error_last);

		    //printf("d_error=%f\n",d_error);
		    p_error_vel_last = p_error_vel;
		  //}
		  // Calculate derivative contribution to command
		  d_term_vel = d * d_error_vel;
		  double cmd = p_term_vel + i_term_vel + d_term_vel;
		  //printf("cmd =%f\n",cmd);
		  return cmd;
	}

	int CompFilter::send_Float_UDP(float* data)
	{
		ss.str("");
		ss<<(*data);
		temp = ss.str();
		strcpy(buf,temp.c_str());
		printf("buf=%s\n",buf);
		rc = sendto(sd, buf, strlen(buf), flags, (struct sockaddr *) &remoteServAddr,sizeof(remoteServAddr));

		if(rc<0) 
		{
		  printf("cannot send data\n");
		  return 0;
		}
		else 
		{
			printf("send data\n");
			for(int p=0;p<20;p++) buf[p]=0;
			usleep(20000); //100ms
			return 1;
		}
	}
	void CompFilter::joyCallback(const sensor_msgs::Joy::ConstPtr& msg)
	{
	   
	   //cout<<"Roll = "<<msg->axes[0]<<"\n"<<"Pitch = "
	   //	<<msg->axes[1]<<"\n"<<"Yaw = "<<msg->axes[2]<<"\n"<<"Height = "<<msg->axes[3]<<endl; 

       /////////////////////////////////////
       // For height joy command
       /////////////////////////////////////
       
	   g_GoalPosition.z = height_cmd_ratio*msg->axes[3]+1.2;
	   //cout<<"Goal Z = "<<g_GoalPosition.z<<endl;


	   /////////////////////////////////////
       // For X axis joy command
       /////////////////////////////////////


#if 1
       // Find the direction of the joy forward cmd
	   if(msg->axes[1] > preJoy_Pitch) joy_forward=TRUE;
	   else joy_forward=FALSE;
	   
	   if ( (joy_forward) &&  (msg->axes[1]>0) && msg->buttons[0])
	   {
	      g_GoalPosition.x -= (msg->axes[1]*JOY_SENSITIVITY_X);
	      if(g_GoalPosition.x < MIN_X) g_GoalPosition.x = MIN_X;
		  else if(g_GoalPosition.x > MAX_X) g_GoalPosition.x = MAX_X;
		  //cout<<"update x forward, goal position = "<<g_GoalPosition.x<<endl;
	   }

	   	


	   if(msg->axes[1] < preJoy_Pitch) joy_backward=TRUE;
	   else joy_backward=FALSE;

	   if ( (joy_backward) &&  (msg->axes[1]<0) && msg->buttons[0])
	   {
	      g_GoalPosition.x -= (msg->axes[1]*JOY_SENSITIVITY_X);
	      if(g_GoalPosition.x < MIN_X) g_GoalPosition.x = MIN_X;
		  else if(g_GoalPosition.x > MAX_X) g_GoalPosition.x = MAX_X;
		  //cout<<"update x backward, goal position = "<<g_GoalPosition.x<<endl;
	   }
#endif


	   if(msg->buttons[0])
	   	{
		  mikoCmd.yaw = (msg->axes[0]*JOY_SENSITIVITY_Y);
		  if(mikoCmd.yaw < MIN_Y) mikoCmd.yaw = MIN_Y;
		  else if(mikoCmd.yaw > MAX_Y) mikoCmd.yaw = MAX_Y;
	   	}
	   else mikoCmd.yaw =0;

	   cout<<"Yaw cmd = "<<mikoCmd.yaw<<endl;

#if 0
	   /////////////////////////////////////
       // For Y axis joy command
       /////////////////////////////////////

	   // Find the direction of the joy forward cmd
	   if(msg->axes[0] > preJoy_Roll) joy_left=TRUE;
	   else joy_left=FALSE;
	   
	   if ( (joy_left) &&  (msg->axes[0]>0) || )
	   {

	      mikoCmd.yaw = (msg->axes[0]*JOY_SENSITIVITY_Y);
		  if(mikoCmd.yaw < MIN_Y) mikoCmd.yaw = MIN_Y;
		  else if(mikoCmd.yaw > MAX_Y) mikoCmd.yaw = MAX_Y;
		  cout<<"Yaw cmd left= "<<mikoCmd.yaw<<endl;
	   }

	   else mikoCmd.yaw=0;


	   if(msg->axes[0] < preJoy_Roll) joy_right=TRUE;
	   else joy_right=FALSE;

	   if ( (joy_right) &&  (msg->axes[0]<0))
	   {
	      mikoCmd.yaw = (msg->axes[0]*JOY_SENSITIVITY_Y);
	      if(mikoCmd.yaw < MIN_Y) mikoCmd.yaw = MIN_Y;
		  else if(mikoCmd.yaw > MAX_Y) mikoCmd.yaw = MAX_Y;
		  cout<<"Yaw cmd right= "<<mikoCmd.yaw<<endl;
	   }

	   preJoy_Pitch = msg->axes[1];
	   preJoy_Roll = msg->axes[0];
#endif

	}		
}
